/***************************************************************************
 创建者: 华磊
 开始时间: 2019.6.13
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file x.h
 *  功能:
 * 改进样条曲线速度规划。
 *
 *                                                                         *
 ***************************************************************************/

#ifndef VELOCITYPROFILE_SIN_H
#define VELOCITYPROFILE_SIN_H
#include "velocityprofile.hpp"
#include "velocityprofile_improvedspline.h"
//enum E_VELOCITY_STYLE
//{
//    E_VELOCITY_STYLE_ZERO_ZERO,
//    E_VELOCITY_STYLE_CHANGE_CONSTANT,
//    E_VELOCITY_STYLE_CONSTANT_CHANGE,
//    E_VELOCITY_STYLE_CHANGE_CONSTANT_ZERO
//};

namespace KDL {
class VelocityProfile_Rectangular;
class VelocityProfile_Spline;

//必须要把父类的纯虚函数实现，才可以new
class VelocityProfile_sin : public VelocityProfile
{
public:
    VelocityProfile_sin(double _maxvel, double _maxacc);

    virtual ~VelocityProfile_sin();

    virtual int SetProfile_transsition(double pos1, double vel1, double pos2, double &vel2, bool isStartChange);//适用于速度过渡

    virtual void SetProfile(double pos1,double pos2);//适用E_VELOCITY_STYLE_ZERO_ZERO
    virtual void SetProfile_ZeroDuration(double pos1,double pos2, double newduration);//

    virtual void SetProfileDuration(double pos1, double pos2, double newduration);//适用E_VELOCITY_STYLE_ZERO_ZERO单一样条曲线

    virtual void SetProfile_tozero(double pos1,double vel1,double pos2);//todo

    /** Compute trapezoidal profile at a given fraction of max velocity
        @param pos1 Position to start from
        @param pos2 Position to end at
        @param newvelocity Fraction of max velocity to use during the
        non-ramp, flat-velocity part of the profile.
        @param KDL::epsilon <= newvelocity <= 1.0 (forcibly clamped to
        this range internally)
    */
    virtual void SetProfileVelocity(
        double pos1,double vel1,double pos2,double vel2,double newvelocity);

    virtual void SetMax(double _maxvel,double _maxacc);
    virtual double Duration() const;
    virtual double Pos(double time) const;
    virtual double Vel(double time) const;//timeRatio！＝１时计算会不对
    virtual double Acc(double time) const;//timeRatio！＝１时计算会不对
    virtual void Write(std::ostream& os) const;
    virtual VelocityProfile *Clone() const;
    // returns copy of current VelocityProfile object. (virtual constructor)


private:
    double duration;//函数内部优先认为是局部变量。
    bool isOneVelProfile;


    // specification of the motion profile :
    double maxvel;
    double maxacc;
    double startpos;
    double startVel;
    double endpos;
    double endVel;
//    E_VELOCITY_STYLE motionStyle;
    //acc=k*sin(c*t) vel=k*(1-cos(c*t))/c  s=k*(t/c-sin(c*t)/2c^2)
    double k1;//k1=maxAcc
    double c1;//c1=2*k1/maxVel
    double t_increaseFinish;
    double t_decreaseStart;
    double s_increaseFinish;
    double s_decreaseStart;
    double realMaxVel;
    double timeRatio;
    bool isNoMoveFlag;
};
}

#endif // VelocityProfile_sin_H
